clc,clear,close all;

%% ---- Build a training set of a similar version of XOR
c_1 = [0 0];
c_2 = [1 1];
c_3 = [0 1];
c_4 = [1 0];

n_L1 = 20; % number of label 1
n_L2 = 20; % number of label 2


A = zeros(n_L1*2, 3);
A(:,3) = 1;
B = zeros(n_L2*2, 3);
B(:,3) = 0;

% create random points
for i=1:n_L1
   A(i, 1:2) = c_1 + rand(1,2)/2;
   A(i+n_L1, 1:2) = c_2 + rand(1,2)/2;
end
for i=1:n_L2
   B(i, 1:2) = c_3 + rand(1,2)/2;
   B(i+n_L2, 1:2) = c_4 + rand(1,2)/2;
end

% show points
scatter(A(:,1), A(:,2),[],'r');
hold on
scatter(B(:,1), B(:,2),[],'g');
X = [A;B];
data = X(:,1:2);
label = X(:,3);

%% Using kmeans to find cinter vector
n_center_vec = 10;
rng(1);
[idx, C] = kmeans(data, n_center_vec);
hold on
scatter(C(:,1), C(:,2), 'b', 'LineWidth', 2);

%% Calulate sigma 
n_data = size(X,1);

% calculate K
K = zeros(n_center_vec, 1);
for i=1:n_center_vec
   K(i) = numel(find(idx == i)); 
end

% Using knnsearch to find K nearest neighbor points for each center vector
% then calucate sigma
sigma = zeros(n_center_vec, 1);
for i=1:n_center_vec
    [n, d] = knnsearch(data, C(i,:), 'k', K(i));
    L2 = (bsxfun(@minus, data(n,:), C(i,:)).^2);
    L2 = sum(L2(:));
    sigma(i) = sqrt(1/K(i)*L2);
end

%% Calutate weights
% kernel matrix
k_mat = zeros(n_data, n_center_vec);

for i=1:n_center_vec
   r = bsxfun(@minus, data, C(i,:)).^2;
   r = sum(r,2);
   k_mat(:,i) = exp((-r.^2)/(2*sigma(i)^2));
end

W = pinv(k_mat'*k_mat)*k_mat'*label;
y = k_mat*W;
%y(y>=0.5) = 1;
%y(y<0.5) = 0;

%% training function and predict function
[W1, sigma1, C1] = RBF_training(data, label, 10);
y1 = RBF_predict(data, W, sigma, C1);
[W2, sigma2, C2] = lazyRBF_training(data, label, 2);
y2 = RBF_predict(data, W2, sigma2, C2);
